#include "hins/xingsong_driver.h"

// #define DEBUG

namespace hins {

XingSongDriver::XingSongDriver(const ConnectionAddress &conn_info,hins::XingSongLaserParam param)
  : conn_info_(conn_info)
  , data_receiver_ptr_(nullptr)
  , laser_param(param)
{
  guard_thread_ = std::thread(&XingSongDriver::RunMain, this);
  shadows_filter_param_.max_angle = -1.0;
}

XingSongDriver::XingSongDriver(const ConnectionAddress &conn_info, XingSongLaserParam param, ShadowsFilterParam shadows_filter_param)
  : conn_info_(conn_info)
  , data_receiver_ptr_(nullptr)
  , laser_param(param)
  , shadows_filter_param_(shadows_filter_param)
{ 
  guard_thread_ = std::thread(&XingSongDriver::RunMain, this);
}

XingSongDriver::~XingSongDriver()
{
  if(data_receiver_ptr_)
  {
    delete data_receiver_ptr_;
  }
}

bool XingSongDriver::StartCapturingTCP()
{
  if(data_receiver_ptr_)
  {
    delete data_receiver_ptr_;
  }

  data_receiver_ptr_ = new LaserDataReceiver(conn_info_);
  if(!data_receiver_ptr_->IsConnected())
  {
    return false;
  }
  data_receiver_ptr_->SyncWrite(data_receiver_ptr_->GenerateParamCommand(laser_param));
  this_thread::sleep_for(chrono::milliseconds(300));                                             //睡眠0.1秒
  if(data_receiver_ptr_->SyncWrite() > 0)
  {
    return true;
  }
  return false;
}


/*
* 防拖尾过滤参数初始化
*/
void XingSongDriver::ShadowsFilterInit(int scan_num)
{
  shadows_filter_threshold_max_.clear();
  shadows_filter_threshold_min_.clear();

  float max, min, min_angle, max_angle, angle_increment;
  angle_increment = 2*M_PI/(float)scan_num;
  int window;

  switch (shadows_filter_param_.shadows_filter_level)
  {
    case 1:                                         // 快速，筛选角度大,搜索窗口小
      min_angle = M_PI*5.0f/180.0f;                 // 转换为弧度制
      max_angle = M_PI*175.0f/180.0f;               // 转换为弧度制
      window = 1;
      break;

    case 2:                                         // 较慢，筛选角度大，搜索窗口适中
      min_angle = M_PI*5.0f/180.0f;                 // 转换为弧度制
      max_angle = M_PI*175.0f/180.0f;               // 转换为弧度制
      window = 3;
      break;

    case 3:                                         // 较慢，筛选角度较小,搜索窗口大
      min_angle = M_PI*15.0f/180.0f;                // 转换为弧度制
      max_angle = M_PI*165.0f/180.0f;               // 转换为弧度制
      window = 5;
      break;
    
    default:                                                           // 按照shadows_filter_param_配置
      min_angle = M_PI*shadows_filter_param_.min_angle/180.0f;         // 转换为弧度制
      max_angle = M_PI*shadows_filter_param_.max_angle/180.0f;         // 转换为弧度制
      window = shadows_filter_param_.window;

      break;
  }
#ifdef DEBUG
  std::cout << "shadows_filter_level:" << shadows_filter_param_.shadows_filter_level
      << "  min_angle:" << shadows_filter_param_.min_angle
      << "  max_angle:" << shadows_filter_param_.max_angle
      << std::endl;
  std::cout << "max:";
#endif
  for(int i=0;i < window;i++)
  {
    max = sin(max_angle)/sin(M_PI-max_angle-angle_increment*(i+1));
    min = sin(min_angle)/sin(M_PI-min_angle-angle_increment*(i+1));

    shadows_filter_threshold_max_.push_back(max);
    shadows_filter_threshold_min_.push_back(min);
    #ifdef DEBUG
    std::cout << shadows_filter_threshold_max_.back() << "  ";
    #endif
  }
}


/*
* 防拖尾过滤
*/
void XingSongDriver::ShadowsFilter(ScanData& scan_data, int scan_num)
{
  #ifdef DEBUG
  int del_num = 0;
  #endif
  if(GetLaserSteadyTime() > 0)
  {
    ShadowsFilterInit(scan_num);
  }
  int search_index, search_index_tmp, del_index, del_index_tmp;
  float a_b_rate;

  shadows_del_index_.clear();
  // 每traverse_step根激光计算一次
  for (int i = 0; i < scan_num; i += shadows_filter_param_.traverse_step)
  {
    if(scan_data.distance_data[i] > kMaxDistance)                        // 如果search_index_tmp激光超出范围，则跳过
      continue;

    for(search_index = i+1;                                              // 搜索[i,i+window]内是否存在拖影现象
        search_index <= i + shadows_filter_param_.window;
        search_index++)
    {
      search_index_tmp = search_index;  

      // 环形索引优化                                                         
      if(search_index_tmp < 0)                                                                        
        search_index_tmp = scan_num + search_index_tmp;
      else
        if(search_index_tmp >= scan_num)
          search_index_tmp = search_index_tmp - scan_num;

      if( (scan_data.distance_data[search_index_tmp] > kMaxDistance) ||                               // 如果search_index_tmp激光超出范围，则跳过
          (scan_data.distance_data[i] > kMaxDistance)                  )
        continue;

      a_b_rate = (float)scan_data.distance_data[i]/(float)scan_data.distance_data[search_index_tmp];  // i与search_index激光比较
      if( (a_b_rate < shadows_filter_threshold_min_[abs(search_index_tmp-i-1)]) ||                      // 如果存在拖影现象
          (a_b_rate > shadows_filter_threshold_max_[abs(search_index_tmp-i-1)])   )        
        if (scan_data.distance_data[i] < scan_data.distance_data[search_index_tmp])
        {
          #ifdef DEBUG
          del_num++;
          #endif
          // scan_data.distance_data[search_index_tmp] = kMaxDistance+1;
          shadows_del_index_.insert(search_index_tmp);
        }
        else
        {
          #ifdef DEBUG
          del_num++;
          #endif
          // scan_data.distance_data[i] = kMaxDistance + 1;
          shadows_del_index_.insert(i);
        }
    }
  }
  for (auto iter = shadows_del_index_.begin(); iter != shadows_del_index_.end(); ++iter) 
  {
    scan_data.distance_data[*iter] = kMaxDistance + 1;
  }
  #ifdef DEBUG
  std::cout << "del_num:" << del_num << std::endl;
  #endif
}

ScanData XingSongDriver::GetFullScan()
{
  if (data_receiver_ptr_)
  {
    if (shadows_filter_param_.max_angle < 0 || shadows_filter_param_.shadows_filter_level == 0)
      return data_receiver_ptr_->GetFullScan();
    ScanData fullscan = data_receiver_ptr_->GetFullScan();
    int scan_num = fullscan.distance_data.size();
    ShadowsFilter(fullscan, scan_num);                      // 过滤拖尾点
    return fullscan;
  }
  else
    return ScanData();
}

void XingSongDriver::RunMain()
{
  if(StartCapturingTCP())
  {
    std::cout << "兴颂雷达启动成功." << std::endl;
  }
  else
  {
    std::cout << "兴颂雷达启动失败，请检查网络连接." << std::endl;
  }

  while(true) // @todo: stop loop condition need to modified
  {
    if(IsConnected())
    {
      //
    }
    else
    {
      while(IsConnected()){
        std::cout << "激光雷达断线，正在重新连接 ... " << std::endl;
        if(StartCapturingTCP()){
          std::cout << "激光雷达重新连接成功." << std::endl;
        }
        usleep(1000 * 50); // 50ms
      }
    }

    usleep(1000*25); // 25ms
  }

  Disconnect();
}

}
